import numpy as np
import cv2

# 从 pixel_to_pointcloud 映射恢复2D栅格地图
def restore_image_from_txt(mapping_file_path, output_image_path, image_size):
    # 创建空的图像矩阵，初始化为灰色
    restored_image = np.ones((image_size[1], image_size[0], 3), dtype=np.uint8) * 128  # 灰色背景

    with open(mapping_file_path, 'r') as f:
        lines = f.readlines()

        for line in lines:
            pixel_data = line.strip().split(':')
            pixel_coords = pixel_data[0].split(',')
            x = int(pixel_coords[0])
            y = int(pixel_coords[1])

            color_data = pixel_data[2].split(',')
            if color_data[0] != 'None':  # 检查是否有点云数据
                r = int(float(color_data[0]) * 255)
                g = int(float(color_data[1]) * 255)
                b = int(float(color_data[2]) * 255)
                # 修正坐标恢复时的翻转问题，调整 x 和 y
                restored_image[y, x] = [b, g, r]  # 注意 OpenCV 的颜色顺序为 BGR

    # 保存恢复后的图像
    cv2.imwrite(output_image_path, restored_image)
    print(f"Restored image saved as {output_image_path}")

# 示例使用
if __name__ == '__main__':
    mapping_file_path = 'data/pixel_to_pointcloud_mapping_median.txt'
    output_image_path = 'data/restored_image.png'
    image_size = (140, 470)  # 示例图像尺寸

    restore_image_from_txt(mapping_file_path, output_image_path, image_size)
